/**
 * @file gray_filter.hpp
 * @author fwy (2643660853@qq.com)
 * @brief
 * @version 0.1
 * @date 2025-03-25
 *
 * @copyright Copyright (c) 2025
 *
 */
#pragma once

#include <memory>
#include <string>
#include "std_msgs/msg/int16.hpp"
#include "nav2_msgs/msg/costmap_filter_info.hpp"
#include "nav2_costmap_2d/costmap_filters/costmap_filter.hpp"

namespace nav2_costmap_2d
{

    class GrayFilter : public CostmapFilter
    {
    public:
        GrayFilter();

        void initializeFilter(
            const std::string &filter_info_topic);

        void process(
            nav2_costmap_2d::Costmap2D &master_grid,
            int min_i, int min_j, int max_i, int max_j,
            const geometry_msgs::msg::Pose2D &pose);

        void resetFilter();

        bool isActive();

    private:
        void filterInfoCallback(const nav2_msgs::msg::CostmapFilterInfo::SharedPtr msg);

        void maskCallback(const nav_msgs::msg::OccupancyGrid::SharedPtr msg);

        void changeState(const int state);

        rclcpp::Subscription<nav2_msgs::msg::CostmapFilterInfo>::SharedPtr filter_info_sub_;
        rclcpp::Subscription<nav_msgs::msg::OccupancyGrid>::SharedPtr mask_sub_;

        rclcpp_lifecycle::LifecyclePublisher<std_msgs::msg::Int16>::SharedPtr gray_state_pub_;

        nav_msgs::msg::OccupancyGrid::SharedPtr filter_mask_;

        std::string mask_frame_;
        std::string global_frame_;

        double base_, multiplier_;

        double flip_threshold1_;
        double flip_threshold2_;


        int default_state_;
        int gray_state_;
        int prev_gray_state_;
    };

}
